from tf.transformations import euler_from_quaternion
from math import fmod, pi
# compute angle
def compute_angle_diff(rot1, rot2):
    a = euler_from_quaternion([rot1.x, rot1.y, rot1.z, rot1.w])[2]
    d_a = euler_from_quaternion([rot2.x, rot2.y, rot2.z, rot2.w])[2]
    return (abs(fmod(a - d_a + 5 * pi, 2 * pi) - pi))
# cpmpute angle and dis
def compute_robot_station(old, new):
    x = (old.pose.position.x - new.pose.position.x) * (old.pose.position.x - new.pose.position.x)
    y = (old.pose.position.y - new.pose.position.y) * (old.pose.position.y - new.pose.position.y)
    disc = math.sqrt(x + y)
    angledisc = compute_angle_diff(old.pose.orientation, new.pose.orientation)
    if disc < 0.2 and angledisc < 0.3:
        return 1
    else:
        return 0
#  gzip unzip
def gzip_uncompress(c_data):
    import gzip, binascii, os
    from cStringIO import StringIO
    buf = StringIO(c_data)
    f = gzip.GzipFile(mode='rb', fileobj=buf)
    try:
        r_data = f.read()
    finally:
        f.close()
    return r_data
# gzip zip
def gzip_compress(raw_data):
    import gzip
    from cStringIO import StringIO
    buf = StringIO()
    f = gzip.GzipFile(mode='wb', fileobj=buf)
    try:
        f.write(raw_data)
    finally:
        f.close()
    return buf.getvalue()
# str split with size
def splitStr(str, size):
    return [str[i:i + size] for i in xrange(0, len(str), size)]

